#include "matrix.h"
#include "math.h"
#include "..\macros\assertions.h"

namespace NEngine {

// ================================================================================================
// Overloaded operator functions
// ================================================================================================
Matrix4x4 Matrix4x4::operator*(float s) {
    Matrix4x4 temp = *this;
    temp.m00 = m00 * s;
    temp.m01 = m01 * s;
    temp.m02 = m02 * s;
    temp.m10 = m10 * s;
    temp.m11 = m11 * s;
    temp.m12 = m12 * s;
    temp.m20 = m20 * s;
    temp.m21 = m21 * s;
    temp.m22 = m22 * s;

    return temp;
}

Matrix4x4 Matrix4x4::operator*=(float s) {
    *this = *this * s;
    return *this;
}

Matrix4x4 operator*(float s, Matrix4x4& mat) {
    Matrix4x4 temp = mat;
    temp.m00 = mat.m00 * s;
    temp.m01 = mat.m01 * s;
    temp.m02 = mat.m02 * s;
    temp.m10 = mat.m10 * s;
    temp.m11 = mat.m11 * s;
    temp.m12 = mat.m12 * s;
    temp.m20 = mat.m20 * s;
    temp.m21 = mat.m21 * s;
    temp.m22 = mat.m22 * s;

    return temp;
}

Matrix4x4 operator*=(float s, Matrix4x4& mat) {
    mat = mat * s;
    return mat;
}

Matrix4x4 Matrix4x4::operator*(Matrix4x4& b) {
    Matrix4x4 temp;

    // -- first row
    temp.m00 = m00*b.m00 + m01*b.m10 + m02*b.m20;
    temp.m01 = m00*b.m01 + m01*b.m11 + m02*b.m21;
    temp.m02 = m00*b.m02 + m01*b.m12 + m02*b.m22;

    // -- second row
    temp.m10 = m10*b.m00 + m11*b.m10 + m12*b.m20;
    temp.m11 = m10*b.m01 + m11*b.m11 + m12*b.m21;
    temp.m12 = m10*b.m02 + m11*b.m12 + m12*b.m22;

    // -- third row
    temp.m20 = m20*b.m00 + m21*b.m10 + m22*b.m20;
    temp.m21 = m20*b.m01 + m21*b.m11 + m22*b.m21;
    temp.m22 = m20*b.m02 + m21*b.m12 + m22*b.m22;
    
    // -- fourth row
    temp.m30 = 0.0f;
    temp.m31 = 0.0f;
    temp.m32 = 0.0f;
    temp.m33 = 1.0f;

    return temp;
}

Matrix4x4 Matrix4x4::operator*=(Matrix4x4& b) {
    *this = *this * b;

    return *this;
}

Matrix4x4 Matrix4x4::operator+(Matrix4x4& b) { 
    Matrix4x4 temp;

    // -- first row
    temp.m00 = m00 + b.m00;
    temp.m01 = m01 + b.m01;
    temp.m02 = m02 + b.m02;
    temp.m03 = m03 + b.m03;

    // -- second row
    temp.m10 = m10 + b.m10;
    temp.m11 = m11 + b.m11;
    temp.m12 = m12 + b.m12;
    temp.m13 = m13 + b.m13;

    // -- third row
    temp.m20 = m20 + b.m20;
    temp.m21 = m21 + b.m21;
    temp.m22 = m22 + b.m22;
    temp.m23 = m23 + b.m23;

    // -- fourth row
    temp.m30 = m30 + b.m30;
    temp.m31 = m31 + b.m31;
    temp.m32 = m32 + b.m32;
    temp.m33 = m33 + b.m33;

    return temp;
}

Matrix4x4 Matrix4x4::operator+=(Matrix4x4& b) { 
    Matrix4x4 temp;

    // -- first row
    temp.m00 = m00 + b.m00;
    temp.m01 = m01 + b.m01;
    temp.m02 = m02 + b.m02;
    temp.m03 = m03 + b.m03;

    // -- second row
    temp.m10 = m10 + b.m10;
    temp.m11 = m11 + b.m11;
    temp.m12 = m12 + b.m12;
    temp.m13 = m13 + b.m13;

    // -- third row
    temp.m20 = m20 + b.m20;
    temp.m21 = m21 + b.m21;
    temp.m22 = m22 + b.m22;
    temp.m23 = m23 + b.m23;

    // -- fourth row
    temp.m30 = m30 + b.m30;
    temp.m31 = m31 + b.m31;
    temp.m32 = m32 + b.m32;
    temp.m33 = m33 + b.m33;

    *this = temp;

    return temp;
}

// ==== End of Overloaded Operator functions ======================================================

// ================================================================================================
// Return the transpose of the matrix
// ================================================================================================
Matrix4x4 Matrix4x4::Transpose3x3() {
    return Matrix4x4(m00, m10, m20, 
                     m01, m11, m21,
                     m02, m12, m22,
                     m30, m31, m32);
}

Matrix4x4 Matrix4x4::Transpose4x4() const {
    return Matrix4x4(m00, m10, m20, m30,
                     m01, m11, m21, m31,
                     m02, m12, m22, m32,
                     m03, m13, m23, m33);
}

// ================================================================================================
// Mutator methods
// ================================================================================================
void Matrix4x4::SetRow1(const Vector4& vec) { 
    m00 = vec.x;
    m01 = vec.y;
    m02 = vec.z;
}

void Matrix4x4::SetRow2(const Vector4& vec) { 
    m10 = vec.x;
    m11 = vec.y;
    m12 = vec.z;
}


void Matrix4x4::SetRow3(const Vector4& vec) { 
    m20 = vec.x;
    m21 = vec.y;
    m22 = vec.z;
}

void Matrix4x4::SetRow4(const Vector4& vec) {
    m30 = vec.x;
    m31 = vec.y;
    m32 = vec.z;
}

// ================================================================================================
// Get the cofactor matrix of the rotation and scale portion matrix
// ================================================================================================
Matrix4x4 Matrix4x4::Cofactor() const {
   Matrix4x4 tmat = Identity(); 
   tmat.m00 = Determinant2x2(m11, m12, m21, m22);
   tmat.m01 = -Determinant2x2(m10, m12, m20, m22);
   tmat.m02 = Determinant2x2(m10, m11, m20, m21);
   tmat.m10 = -Determinant2x2(m01, m02, m21, m22);
   tmat.m11 = Determinant2x2(m00, m02, m20, m22);
   tmat.m12 = -Determinant2x2(m00, m01, m20, m21);
   tmat.m20 = Determinant2x2(m01, m02, m11, m12);
   tmat.m21 = -Determinant2x2(m00, m02, m10, m12);
   tmat.m22 = Determinant2x2(m00, m01, m10, m11);

   return tmat;
}

// ================================================================================================
// Get the adjoint matrix of the rotation and scale portion matrix
// ================================================================================================
Matrix4x4 Matrix4x4::Adjoint() {
    Matrix4x4 tmat = Cofactor();
    return tmat.Transpose3x3();
}

// ================================================================================================
// Get the inverse of the rotation and scale portion of the matrix
// ================================================================================================
Matrix4x4 Matrix4x4::Inverse() {
    float det = Determinant3x3();
    ASSERT_(det, "Det is zero, cannot have inverse");

    return (1.0f/det) * Adjoint();
}

Matrix4x4 Matrix4x4::InvTrans() const {
    float det = Determinant3x3();
    ASSERT_(det, "Det is zero, cannot have inverse");

    return (1.0f/det) * Cofactor();
}

// ================================================================================================
// Get the determinant of the 2x2 matrix
// ================================================================================================
float Matrix4x4::Determinant2x2(float a, float b, float c, float d) const {
    return ((a * d) - (b * c));
}

// ================================================================================================
// Get the determinant of the 3x3 matrix
// ================================================================================================
float Matrix4x4::Determinant3x3() const {
    float i = m00 * (Determinant2x2(m11, m12, m21, m22));
    float j = m01 * (Determinant2x2(m10, m12, m20, m22));
    float k = m02 * (Determinant2x2(m10, m11, m20, m21));
    
    return (i - j + k);
}

} // NEngine namespace
